// RIT Robotic Systems 2011-3 Project: Blue Steel
// Matt Hodge and Max Macri
//

#include <Ping.h> 
#include <Movement.h>

 const int stop_inches = 9; // inches before an object to stop
 const int min_forwardTime = 2000; // ms
 const int max_forwardTime = 4001; // ms
 const int min_turnTime = 500; // ms
 const int max_turnTime = 1501; // ms (+mintime)
 
 long randMove = 0;
 long randTurn = 0;
 
 Ping ping = Ping(4,0,29); //Pin,in_offset,cm_offset //74
 Movement move = Movement(6,7,8,9,11,10);
 
void setup() {
  // Begin Debug
  Serial.begin(9600);
  
  // Seed PRNG
  randomSeed(analogRead(0));
}

void loop() {
  int moveTime = 0;
  
  // move forward for a random time (2000-6000ms)
  randMove = random(max_forwardTime);
  randMove += min_forwardTime;
  
  // move forward for the generated time, avoiding objects, then turn
  while(moveTime < randMove){
    ping.fire();
    Serial.print("Inches: ");
    Serial.print(ping.inches());
    Serial.print("\t");
    
    if(ping.inches() <= stop_inches){
      Serial.println("RotateRight");
      move.setMove(25,50);
      // may want to update moveTime and delay() here
      // and use a different time differential than 500ms...
      moveTime = 0;
      
    }else{
      Serial.print("Forward: ");
      Serial.print(moveTime);
      Serial.print(":");
      Serial.print(randMove);
      Serial.print("\n");
      
      move.setMove(50,25);
      moveTime += 500;
    }
    
    delay(500);
  }
  Serial.println("RotateRight");
  randTurn = random(max_turnTime);
  randTurn += min_turnTime;
  
  //moveTime = 0;
  //while(moveTime <= randTurn){
    //move.setMove(100,50);
    //moveTime += 250;
    //delay(250);
  //}
  move.setMove(25,50);
  delay(50);
  move.setMove(25,50);
  delay(randTurn);
  
}
